#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "LED.h"
#include "Key.h"
#include "AD.h"
#include "Motor.h"
#include "Timer.h"
#include "Encoder.h"
#include "Serial.h"
#include "math.h"

float Target, Actual, Out;
float Ki=0,Kp=0,Kd=0;
float Error0, Error1, ErrorInt;

uint16_t KeyNum;


/*测试*/
int main(void)
{
	/*模块初始化*/
    Key_Init();
    Timer_Init();
    Motor_Init();
    Encoder_Init();
	OLED_Init();		//OLED初始化
    RP_Init();
    Serial_Init();
    
    OLED_Printf(0, 0, OLED_8X16, "电机速度控制");
    OLED_Update();
	while (1)
	{ 
//        KeyNum = Key_GetNum();
//        if (KeyNum == 1)
//        {
//            Target += 10;
//        }
//        if (KeyNum == 2)
//        {
//            Target-= 10;
//        }
//        if (KeyNum == 3)
//        {
//            Target = 0;
//        }
        
        Kp = RP_GetValue(1);
        Ki = RP_GetValue(2);
        Kd = RP_GetValue(3);
        Actual = RP_GetValue(4);
        
        OLED_Printf(0, 16, OLED_8X16, "Kp%4.2f", Kp); // +表示始终显示+号，0：前面添0，4：数据宽度四位，.0显示一位小数
        OLED_Printf(0, 32, OLED_8X16, "Ki%4.2d", Ki);
        OLED_Printf(0, 32, OLED_8X16, "Kd%4.2d", Kd);
        
        OLED_Printf(0, 16, OLED_8X16, "Tar%+04.0f", Target); // +表示始终显示+号，0：前面添0，4：数据宽度四位，.0显示一位小数
        OLED_Printf(0, 32, OLED_8X16, "Act%+04d", Actual);
        OLED_Printf(0, 32, OLED_8X16, "Out%+04d", Out);
        OLED_Update();
        Serial_Printf("%f,%f,%f\r\n",Target, Actual, Out);
	}
}

void TIM1_UP_IRQHandler(void)
{
    static uint16_t count = 0;
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
        Key_Tick();
        count ++;
        if (count > 40)
        {
            count = 0;
            
            Actual = Encoder_Get();
            Error1 = Error1;
            Error0 = Target - Actual;
            
            if (Ki != 0)
            {
                ErrorInt  += Error0;
            }
            else
                ErrorInt = 0;

            
            Out = Kp*Error0 + Ki*ErrorInt + Kd*(Error1 - Error0);
            if (Out > 100)Out = 100;
            if (Out < -100)Out = -100;
            Motor_SetPWM(Out);
        }
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
	}
}
